#include "mpu6500.h"

extern Mpu6500_DataTydef mpu_data;
uint8_t rx;
uint8_t tx;

// MPU6500单次写命令
uint8_t mpu_write_byte(uint8_t const reg, uint8_t const data)
{
    MPU_NSS_LOW;                                         //开始通讯
    tx = reg & 0x7F;                                     //使第一位为0（写模式）
    HAL_SPI_TransmitReceive(&MPU_HSPI, &tx, &rx, 1, 55); //写入命令地址
    tx = data;
    HAL_SPI_TransmitReceive(&MPU_HSPI, &tx, &rx, 1, 55); //写入数据
    MPU_NSS_HIGH;                                        //结束通讯
    return 0;
}
// MPU6500单次读取单字节数据
uint8_t mpu_read_byte(uint8_t const reg)
{
    MPU_NSS_LOW;
    tx = reg | 0x80;                                     //使地址第一位为1（读模式）
    HAL_SPI_TransmitReceive(&MPU_HSPI, &tx, &rx, 1, 55); //写入需要读取的地址
    HAL_SPI_TransmitReceive(&MPU_HSPI, &tx, &rx, 1, 55); //为读取的数据提供存储空间
    MPU_NSS_HIGH;
    return rx;
}
// MPU6500初始化
uint8_t Mpu_Init(void)
{
    HAL_Delay(100);

    // uint8_t id = mpu_read_byte(MPU6500_WHO_AM_I);
    int i = 0;
    uint8_t MPU6500_Init_Data[10][2] = {
        {MPU6500_PWR_MGMT_1, 0x80},     /* Reset Device */
        {MPU6500_PWR_MGMT_1, 0x03},     /* Clock Source - Gyro-Z */
        {MPU6500_PWR_MGMT_2, 0x00},     /* Enable Acc & Gyro */
        {MPU6500_CONFIG, 0x04},         /* LPF 41Hz */
        {MPU6500_GYRO_CONFIG, 0x18},    /* +-2000dps */
        {MPU6500_ACCEL_CONFIG, 0x10},   /* +-8G */
        {MPU6500_ACCEL_CONFIG_2, 0x02}, /* enable LowPassFilter  Set Acc LPF */
        {MPU6500_USER_CTRL, 0x20},      /* Enable AUX */
    };
    for (i = 0; i < 10; i++)
    {
        mpu_write_byte(MPU6500_Init_Data[i][0], MPU6500_Init_Data[i][1]);
        HAL_Delay(1);
    }

    //误差补偿值置0
    mpu_data.MPU6500_GYRO_X_OFFSET = 0;
    mpu_data.MPU6500_GYRO_Y_OFFSET = 0;
    mpu_data.MPU6500_GYRO_Z_OFFSET = 0;

    mpu_data.MPU6500_ACCEL_X_OFFSET = 0;
    mpu_data.MPU6500_ACCEL_Y_OFFSET = 0;
    mpu_data.MPU6500_ACCEL_Z_OFFSET = 0;

    //初始化四元数
    mpu_data.q[0] = 1;
    mpu_data.q[1] = 0;
    mpu_data.q[2] = 0;
    mpu_data.q[3] = 0;
    //误差累积积分置零
    mpu_data.exint = 0;
    mpu_data.eyint = 0;
    mpu_data.ezint = 0;
    return 0;
}
void Get_MPU6500_Data(void)
{
    /* 加速度数据 1 -> m/s^2   量程 32768 ->8g */
    mpu_data.ax = Get_16Bit_Data(MPU6500_ACCEL_XOUT_H, MPU6500_ACCEL_XOUT_L) / 417.676f + mpu_data.MPU6500_ACCEL_X_OFFSET;
    mpu_data.ay = Get_16Bit_Data(MPU6500_ACCEL_YOUT_H, MPU6500_ACCEL_YOUT_L) / 417.676f + mpu_data.MPU6500_ACCEL_Y_OFFSET;
    mpu_data.az = Get_16Bit_Data(MPU6500_ACCEL_ZOUT_H, MPU6500_ACCEL_ZOUT_L) / 417.676f + mpu_data.MPU6500_ACCEL_Z_OFFSET;

    /* 陀螺仪数据  2000dps -> °/s */
    mpu_data.gx = Get_16Bit_Data(MPU6500_GYRO_XOUT_H, MPU6500_GYRO_XOUT_L) / 938.734f + mpu_data.MPU6500_GYRO_X_OFFSET;
    mpu_data.gy = Get_16Bit_Data(MPU6500_GYRO_YOUT_H, MPU6500_GYRO_YOUT_L) / 938.734f + mpu_data.MPU6500_GYRO_Y_OFFSET;
    mpu_data.gz = Get_16Bit_Data(MPU6500_GYRO_ZOUT_H, MPU6500_GYRO_ZOUT_L) / 938.734f + mpu_data.MPU6500_GYRO_Z_OFFSET;
    //温度 摄氏度
    mpu_data.temp = Get_16Bit_Data(MPU6500_TEMP_OUT_H, MPU6500_TEMP_OUT_L) / 333.87f + 21;
}

float Get_16Bit_Data(uint8_t addr_h, uint8_t addr_l)
{
    static uint8_t buf[2];
    static short data;

    buf[0] = mpu_read_byte(addr_l);
    buf[1] = mpu_read_byte(addr_h);
    data = (buf[1] << 8) | buf[0];

    return data;
}
void Mpu_Self_Test()
{
    float sum_gx_sample;
    float sum_gy_sample;
    float sum_gz_sample;
    int i;

    for (i = 0; i < NUM_SAMPLE; i++)
    {
        Get_MPU6500_Data();

        sum_gx_sample += mpu_data.gx;
        sum_gy_sample += mpu_data.gy;
        sum_gz_sample += mpu_data.gz;
        HAL_Delay(1);
    }

    mpu_data.MPU6500_GYRO_X_OFFSET += -sum_gx_sample / NUM_SAMPLE;
    mpu_data.MPU6500_GYRO_Y_OFFSET += -sum_gy_sample / NUM_SAMPLE;
    mpu_data.MPU6500_GYRO_Z_OFFSET += -sum_gz_sample / NUM_SAMPLE;
    mpu_data.MPU6500_ACCEL_X_OFFSET = 1.0f;
    mpu_data.MPU6500_ACCEL_Y_OFFSET = 2.6f;
    mpu_data.MPU6500_ACCEL_Z_OFFSET = 0.6f;
    HAL_Delay(100);
}
